#ifndef DISCHANDLING_H
#define DISCHANDLING_H
#include "Commands/Subsystem.h"
#include "WPILib.h"

/**
 *
 *
 * @author team308
 */

typedef void (*sensorHook)(int);

class DiscHandling: public Subsystem {
private:
	// It's desirable that everything possible under private except
	// for methods that implement subsystem capabilities
	static const int sensorFilterRange = 20;
	static const int sensorFilterHalf = 10;
	int sensorCount[4];
	char sensorValue[4];
	DigitalInput* sensors[4];
public:
	
	bool AUTO;
	enum SensorID {
		bot = 0,
		mid = 1,
		top = 2,
		breach = 3
	};
	Notifier* proxUpdater;
	Notifier* extendPin;
	Notifier* retractPin;
	Notifier* startConveyor;
	Notifier* stopConveyor;
	
	Timer pinTimer;
	Victor conveyorVictor;
	Solenoid conveyorPin;
	DigitalInput breachProxSensor;
	DigitalInput topProxSensor;
	DigitalInput middleProxSensor;
	DigitalInput bottomProxSensor;
	enum ConveyorDirection{
		kConveyorUp,
		kConveyorDown,
		kConveyorOff
	};
	DiscHandling();
	static void UpdateSensors(void*);
	static void ExtendPin(void*);
	static void RetractPin(void*);
	static void PulsePin(void *);
	bool GetSensor(SensorID sensor);
	static void StopConveyor(void*);
	static void StartConveyor(void*);
	void InitDefaultCommand();
	void ControlConveyorState(ConveyorDirection Direction);
	void ControlPinState(bool IsOut);
	int GetDiscNumber();
	bool GetIsConveyorUp();
};

#endif
